/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       can_receive.c/h
  * @brief      there is CAN interrupt function  to receive motor data,
  *             and CAN send function to send motor current to control motor.
  *             这里是CAN中断接收函数，接收电机数据,CAN发送函数发送电机电流控制电机.
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *  V1.1.0     Nov-11-2019     RM              1. support hal lib
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */

#include "CAN_communication.h"

#include "bsp_rng.h"
#include "cmsis_os.h"
#include "main.h"
#include "math.h"
#include "string.h"
#include "usb_task.h"

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;

#define CAN_1 hcan1
#define CAN_2 hcan2

static CAN_TxHeaderTypeDef TX_MESSAGE;
static uint8_t CAN_SEND_DATA[8];
static PostureInfo_t POSTURE_INFO;

// Function declaration
static void CANRxDecode(CAN_RxHeaderTypeDef * rx_header, uint8_t rx_data[8]);

/**
 * @brief          发送控制信号
 * @param[in]      can_handle 选择CAN1或CAN2
 * @param[in]      tx_header  CAN发送数据header
 * @param[in]      tx_data    发送数据
 * @return         none
 */
static void CAN_SendTxMessage(
    CAN_HandleTypeDef * can_handle, CAN_TxHeaderTypeDef * tx_header, uint8_t * tx_data)
{
    uint32_t send_mail_box;

    uint32_t free_tx_mailbox = HAL_CAN_GetTxMailboxesFreeLevel(can_handle);  // 检测是否有空闲邮箱
    while (free_tx_mailbox < 3) {  // 等待空闲邮箱数达到3
        free_tx_mailbox = HAL_CAN_GetTxMailboxesFreeLevel(can_handle);
    }
    HAL_CAN_AddTxMessage(can_handle, tx_header, tx_data, &send_mail_box);
}

/**
 * @brief          通过CAN发送数据
 * @param[in]      std_id 发送数据使用的std_id
 * @param[in]      data1 发送数据
 * @param[in]      data2 发送数据
 * @return         none
 */
void CanSendData(can_msg_id_e std_id, float data1, float data2)
{
    TX_MESSAGE.StdId = std_id;
    TX_MESSAGE.IDE = CAN_ID_STD;
    TX_MESSAGE.RTR = CAN_RTR_DATA;
    TX_MESSAGE.DLC = 0x08;

    memcpy(&CAN_SEND_DATA[0], &data1, sizeof(float));
    memcpy(&CAN_SEND_DATA[4], &data2, sizeof(float));

    CAN_SendTxMessage(&CAN_1, &TX_MESSAGE, CAN_SEND_DATA);
}

/**
 * @brief          hal库CAN回调函数,接收数据
 * @param[in]      hcan:CAN句柄指针
 * @retval         none
 */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef * hcan)
{
    CAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[8];

    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);  // 获取CAN数据
    CANRxDecode(&rx_header, rx_data);                               // CAN数据解码
}

/**
 * @brief          反馈数据解码
 * @retval         none
 */
static void CANRxDecode(CAN_RxHeaderTypeDef * rx_header, uint8_t rx_data[8])
{
#if (SELF_C_BOARD == C_BOARD_MAIN_ID)
    if (rx_header->IDE == CAN_ID_STD) {
        switch (rx_header->StdId) {
            case BIG_ARM_ID: {
                memcpy(&POSTURE_INFO.yaw, &rx_data[0], sizeof(float));
                memcpy(&POSTURE_INFO.big_arm_pitch, &rx_data[4], sizeof(float));
            } break;
            case SMALL_ARM_ID: {
                memcpy(&POSTURE_INFO.small_arm_roll, &rx_data[0], sizeof(float));
                memcpy(&POSTURE_INFO.small_arm_pitch, &rx_data[4], sizeof(float));
            } break;
            default:
                break;
        }
    }
#endif
    return;
}

PostureInfo_t * GetPostureInfoPoint(void) { return &POSTURE_INFO; }
